global !p
from os.path import expanduser, join
import sys; sys.path.append(join(expanduser("~"), ".mcf/scripts/library"))
from conversions import *
def get_level(c):
  levels = ["DEBUG", "INFO", "WARN", "ERROR", "FATAL"]
  for l in levels:
    if l.lower().startswith(c.lower()):
      return l
  return "INFO"
endglobal

snippet "log(\w?)(s?)" "Log message" br
ROS_`!p
snip.rv = get_level(match.group(1) or 'i')
if match.group(2):
  snip.rv += '_STREAM'
`("${1:Message}"`!p
if match.group(2):
  snip.rv = ' << '
`$2);$0
endsnippet

snippet ".par(d?)" "Query parameter from server" !wr
`!p snip.rv = '.param' if match.group(1) else '.getParam'
`${4/.+/</}${4:std::string}${4/.+/>/}("${1:parameter_name}", ${2:$1}${3/.+/, /}${3:default}${3/.+//})$0
endsnippet

snippet dynrec "Dynamic reconfigure template" !b
#include <dynamic_reconfigure/server.h>
#include <$1/$2.h>
void ${4:reconfigureCallback}($1::$2 &config, uint32_t level)
{
  $0
}
dynamic_reconfigure::Server<${1:package}::${2:FooConfig}> ${3:server};
$3.setCallback(boost::bind(&$4, _1, _2));
endsnippet

snippet "acli(ent)?" "Action client" !br
#include <actionlib/client/simple_action_client.h>
#include <$1/$2.h>
actionlib::SimpleActionClient<${1:package}::${2:FooAction}> ${3:client}(${4:"topic"}, true);
endsnippet

snippet acall "Action call" !b
${1:package::FooGoal} ${2:goal};
$2.${3:/* fill in goal parameters */}
${4:client}.sendGoal($2);
if ($4.waitForResult(ros::Duration(${5:30.0})))
{
  actionlib::SimpleClientGoalState state = $4.getState();
}
endsnippet

snippet cbm "Callback for messages" !b
void ${1:callback}(const ${2:std_msgs::String}ConstPtr& ${3:msg})
{
  $0
}
endsnippet

snippet cbs "Callback for services" !b
bool ${1:callback}(${2:std_srvs::Empty}::Request& ${3:request}, $2::Response& ${4:response})
{
  $0
  return true;
}
endsnippet


###########################################################################
#                                  Node                                   #
###########################################################################

snippet init "Initialize node" !b
ros::init(argc, argv, "${1:`!v Filename("$1")`}");$0
endsnippet

snippet node "Node class declaration" !b
class ${1:`!p snip.rv = snakecase_to_camelcase(snip.basename) + "Node"`}
{

public:

  ${1/(\w+).*/$1/}(${2})
  {
    $0
  }

private:

  ros::NodeHandle nh_;

};
endsnippet

snippet nh "Node handle" !b
ros::NodeHandle ${1:nh}$0
endsnippet

snippet pn "Private node handle" !b
ros::NodeHandle ${1:pn}("~")$0
endsnippet


###########################################################################
#                          Publisher/Subscriber                           #
###########################################################################

snippet pub "Publisher" !b
${1/.*?(_?$)/(?1::ros\:\:Publisher )/}${1:publisher} = ${2:nh}${1/.*?(_?$)/(?1:_:)/}.advertise<${3:std_msgs::String}>("${4:topic}", ${5:10});$0
endsnippet

snippet sub "Subscriber" !b
${1/.*?(_?$)/(?1::ros\:\:Subscriber )/}${1:subscriber} = ${2:nh}${1/.*?(_?$)/(?1:_:)/}.subscribe("${3:topic}", ${4:10}, ${5:callback}${5/.+?(C?$)/(?1:allback:)/}${5/(^&?).*/(?1:, this:)/});$0
endsnippet


###########################################################################
#                                Services                                 #
###########################################################################

snippet "scli(ent)?" "Service client" !br
${1/.*?(_?$)/(?1::ros\:\:ServiceClient )/}${1:client} = ${2:nh}${1/.*?(_?$)/(?1:_:)/}.serviceClient<${3:std_srvs::Empty}>("${4:topic}");
$3 ${5:srv};
$5.request.${6:/* fill in request parameters */}
if ($1.call($5))
{
  $0
}
endsnippet

snippet "s(srv|server)" "Service server" !br
${1/.*?(_?$)/(?1::ros\:\:ServiceServer )/}${1:service} = ${2:nh}${1/.*?(_?$)/(?1:_:)/}.advertiseService("${3:topic}", ${4:callback}${4/.+?(C?$)/(?1:allback:)/}${4/(^&?).*/(?1:, this:)/})$0
endsnippet

snippet scall "Service call (without creating client)" !b
${1:std_srvs::Empty} ${2:srv};
$2.request.${4:/* fill in request parameters */}
if (ros::service::call("${3:topic}", $2))
{
  $0
}
endsnippet


###########################################################################
#                             Transformations                             #
###########################################################################
# http://ros.org/doc/fuerte/api/tf/html/c++/namespacetf.html

snippet y2qm "Yaw to quaternion message"
${1:geometry_msgs::Quaternion ${2:q} = }tf::createQuaternionMsgFromYaw(${3:yaw})$0
endsnippet

snippet y2q "Yaw to quaternion"
${1:tf::Quaternion ${2:q} = }tf::createQuaternionFromYaw(${3:yaw})$0
endsnippet

snippet q2y "Quaternion to yaw"
${1:double ${2:yaw} = }tf::getYaw(${3:q})$0
endsnippet

snippet e2qm "Euler to quaternion message"
${1:geometry_msgs::Quaternion ${2:q} = }tf::createQuaternionMsgFromRollPitchYaw(${3:roll}, ${4:pitch}, ${5:yaw})$0
endsnippet

snippet e2q "Euler to quaternion"
${1:tf::Quaternion ${2:q} = }tf::createQuaternionFromRPY(${3:roll}, ${4:pitch}, ${5:yaw})$0
endsnippet

snippet tfl "Transform lookup" !b
#include <tf/transform_listener.h>
tf::TransformListener $2;
tf::StampedTransform ${1:transform};
try
{
  ros::Time now = ros::Time::now();
  ${2:tf_listener_}.waitForTransform("${3:odom}", "${4:base_link}", now, ros::Duration(${5:0.1}));
  $2.lookupTransform("$3", "$4", now, $1);
}
catch (tf::TransformException& e)
{
  ROS_ERROR("Unable to lookup the transform: %s", e.what());
}
endsnippet


###########################################################################
#                                  Time                                   #
###########################################################################

snippet now "Current time"
ros::Time::now()
endsnippet


###########################################################################
#                                Messages                                 #
###########################################################################

snippet wfm "Wait for message" !b
auto ${1:msg} = ros::topic::waitForMessage<${2:std_msgs::String}>(${3:"topic"}${4:, ros::Duration(${5:timeout})});
endsnippet
